/*
 * Copyright (c) 2010, Jim Hollinger
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of Jim Hollinger nor the names of its contributors
 *     may be used to endorse or promote products derived from this
 *     software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <iomanip>
#define _USE_MATH_DEFINES
#include <math.h>

#include "debughelper/debughelper.h"
#include "nmea0183gga.h"
#include "nmea0183rmc.h"
#include "nmea0183parser.h"

#if !defined (KNOTS_TO_METERS_PER_SECOND)
#  define KNOTS_TO_METERS_PER_SECOND     (0.514444444F)
#endif


using namespace nmea0183;


const char Nmea0183Parser::NMEA0183_START_CHAR     = '$';
const char Nmea0183Parser::NMEA0183_COMMA_CHAR     = ',';
const char Nmea0183Parser::NMEA0183_CHECKSUM_CHAR  = '*';
const char Nmea0183Parser::NMEA0183_END_CHAR1      = 0x0D;
const char Nmea0183Parser::NMEA0183_END_CHAR2      = 0x0A;

Nmea0183Parser::Nmea0183Parser() : chars(NULL), params(NULL) {
    valid_flag   = false;
    state       = NMEA0183_PARSER_NEED_START;

    maxNumChars  = NMEA0183_MAX_SENTENCE_LEN;
    numChars     = 0;
    chars        = new char[maxNumChars];

    if ((chars != NULL)) {
        valid_flag = true;
        maxParamLen  = 128;
        maxNumParams = 64;
        numParams    = 0;
        params       = new char *[maxNumParams];
        if (params == NULL) {
            valid_flag = false;
        } else {
            for (int i = 0; i < maxNumParams; i++) {
                params[i] = new char[maxParamLen];
                if (params[i] == NULL) {
                    valid_flag = false;
                } else {
                    params[i][0] = '\0';
                }
            }
        }
    }
}

Nmea0183Parser::~Nmea0183Parser() {
    bool flag = true;
    do {
        Nmea0183Message *msg = getNextMessage();
        if (msg == NULL) {
            flag = false;
        } else {
            deleteMessage(msg);
            msg = NULL;
        }
    } while (flag);
    if (params != NULL) {
        for (int i = maxNumParams - 1; i >= 0; i--) {
            if (params[i] != NULL) {
                delete []params[i];
                params[i] = NULL;
            }
        }
        delete []params;
        params = NULL;
    }
    delete []chars;
    chars = NULL;
}

bool Nmea0183Parser::parse(const char *data) {
    int len = strnlen(data, NMEA0183_MAX_SENTENCE_LEN);
    return parse(data, len);
}

bool Nmea0183Parser::parse(const char *data, int len) {
    bool parsed_flag = false;

    // append new data
    int freeChars = maxNumChars - numChars;
    if (freeChars < len) {

        // make room for new data (an overflow has occured)
        int n = len - freeChars;
        memmove(&chars[0], &chars[n], maxNumChars - n);
        numChars -= n;
    }
    memcpy(&chars[numChars], data, len);
    numChars += len;

    // switch on state
    bool done_flag = false;
    while (!done_flag) {
        switch (state) {

        case NMEA0183_PARSER_NEED_START:
        default:
            {
                for (int i = 0; i < numChars; i++) {
                    if (chars[i] == NMEA0183_START_CHAR) {
                        memmove(&chars[0], &chars[i], numChars - i);
                        numChars -= i;
                        state = NMEA0183_PARSER_NEED_1ST_COMMA;
                        break;
                    }
                }
                if (state == NMEA0183_PARSER_NEED_START) {
                    numChars = 0;
                    done_flag = true;
                }
            }
            break;

        case NMEA0183_PARSER_NEED_1ST_COMMA:
            if (numChars >= 7) {
                if (chars[6] == NMEA0183_COMMA_CHAR) {
                    state = NMEA0183_PARSER_NEED_END;
                } else {
                    memmove(&chars[0], &chars[1], numChars - 1);
                    numChars -= 1;
                    state = NMEA0183_PARSER_NEED_START;
                }
            }
            break;

        case NMEA0183_PARSER_NEED_END:
            {
                for (int i = 7; i < numChars; i++) {
                    if (chars[i] == NMEA0183_END_CHAR1) {
                        if ((i + 1) < numChars) {
                            if (chars[i + 1] == NMEA0183_END_CHAR2) {
                                chars[i] = '\0';
                                if (verifyChecksum(chars)) {
                                    if (parseSentence(chars)) {
                                        parsed_flag = parseParams();
                                    }
                                }

                                i += 2;
                                memmove(&chars[0], &chars[i], numChars - i);
                                numChars -= i;
                                state = NMEA0183_PARSER_NEED_START;
                                break;
                            }
                        }
                    } else if (chars[i] == NMEA0183_START_CHAR) {
                        memmove(&chars[0], &chars[i], numChars - i);
                        numChars -= i;
                        state = NMEA0183_PARSER_NEED_1ST_COMMA;
                        break;
                    }
                }
                if (state == NMEA0183_PARSER_NEED_END) {
                    done_flag = true;
                }
            }
            break;
        }
    }

    return parsed_flag;
}

bool Nmea0183Parser::parseSentence(const char *sentence) {
    numParams = 0;
    bool done_flag = false;
    int j = 0;
    for (int i = 0; (i < NMEA0183_MAX_SENTENCE_LEN) && !done_flag; i++) {
        char c = sentence[i];
        switch (c) {

        case NMEA0183_COMMA_CHAR:
        case NMEA0183_CHECKSUM_CHAR:
        case NMEA0183_END_CHAR1:
        case NMEA0183_END_CHAR2:
        case '\0':
            params[numParams][j] = '\0';
            j = 0;
            if (++numParams >= maxNumParams) {
                done_flag = true;
            }
            if ((c == NMEA0183_CHECKSUM_CHAR) ||
                (c == NMEA0183_END_CHAR1)     ||
                (c == NMEA0183_END_CHAR2)     ||
                (c == '\0')) {
                done_flag = true;
            }
            break;

        default:
            if (j < (maxParamLen - 1)) {
                params[numParams][j++] = c;
            } else {
                params[numParams][maxParamLen - 1] = '\0';
            }
            break;
        }
    }

    return true;
}

bool Nmea0183Parser::parseParams() {
    bool flag = false;

    if (numParams > 0) {
        if (strnlen(params[0], maxParamLen) == NMEA0183_TALKER_MSG_TYPE_LEN) {

            // determine which particular message this is
            Nmea0183Message *msg = NULL;
            const char *msgType = &params[0][1 + NMEA0183_TALKER_LEN];
            if (strncmp(msgType, "GGA", NMEA0183_MSG_TYPE_LEN) == 0) {
                msg = new Nmea0183GGA();
            } else if (strncmp(msgType, "GSA", NMEA0183_MSG_TYPE_LEN) == 0) {
                flag = true;  // GSA message type ignored
            } else if (strncmp(msgType, "GSV", NMEA0183_MSG_TYPE_LEN) == 0) {
                flag = true;  // GSV message type ignored
            } else if (strncmp(msgType, "RMC", NMEA0183_MSG_TYPE_LEN) == 0) {
                msg = new Nmea0183RMC();
            } else {
                std::cerr << "NMEA0183 Unrecognized message type: " <<
                    msgType <<std::endl;
            }
            if (msg != NULL) {

                // parse the paramaters for the particular message
                flag = msg->parse(const_cast <const char **> (params),
                                  numParams);
                if (flag) {

                    // add message to parsed queue
                    msgQueue.push(msg);
                } else {
                    delete msg;
                    msg = NULL;
                }
            }
        }
    }

    return flag;
}

Nmea0183Message *Nmea0183Parser::getNextMessage() {
    Nmea0183Message *msg = NULL;

    if (!msgQueue.empty()) {
        msg = msgQueue.front();
        msgQueue.pop();
    }

    return msg;
}

void Nmea0183Parser::deleteMessage(Nmea0183Message *msg) {
    delete msg;
    msg = NULL;
}

void Nmea0183Parser::printParams() {
    printParams(const_cast <const char **> (params), numParams);
}

void Nmea0183Parser::printParams(const char **params, int numParams) {
    if (numParams > 0) {
        std::cout << "NMEA0183 " << params[0] << std::endl;
    }
    for (int i = 1; i < numParams; i++) {
        std::cout << std::setw(2) << i << ": " << params[i] << std::endl;
    }
}

unsigned int Nmea0183Parser::computeChecksum(const char *sentence) {
    unsigned char checksum = 0;

    for (int i = 0; i < NMEA0183_MAX_SENTENCE_LEN; i++) {
        if (sentence[i] == NMEA0183_START_CHAR) {
            // ignore
        } else if (sentence[i] == NMEA0183_CHECKSUM_CHAR) {
            break;
        } else if (sentence[i] == '\0') {
            break;
        } else {
            checksum ^= (unsigned char) sentence[i];
        }
    }

    return checksum;
}

bool Nmea0183Parser::verifyChecksum(const char *sentence) {
    bool flag = false;

    int len = strnlen(sentence, NMEA0183_MAX_SENTENCE_LEN);
    if (sentence[len - 3] == NMEA0183_CHECKSUM_CHAR) {
        unsigned int computedChecksum = computeChecksum(sentence);
        unsigned int sentenceChecksum = strtoul(&sentence[len - 2], NULL, 16);
        flag = (computedChecksum == sentenceChecksum);
        if (!flag) {
            std::cerr << "Nmea0183Parser.verifyChecksum 0x";
            std::cerr << std::noshowbase << std::uppercase << std::setw(2) <<
                std::setfill('0') << std::hex << computedChecksum;
            std::cerr << " != 0x";
            std::cerr << std::noshowbase << std::uppercase << std::setw(2) <<
                std::setfill('0') << std::hex << sentenceChecksum;
            std::cerr << " in \"" << sentence << "\"" <<std::endl;
        }
    } else {
        // skip checksum verification if checksum character not present
        flag = true;
    }

    return flag;
}

// http://www.gpsinformation.org/dale/nmea.htm
bool Nmea0183Parser::unitTest() {
    bool flag = true;

    Nmea0183Parser nmea;

    if (!nmea.parse("$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47\r\n"))       flag = false;
    if (!nmea.parse("$GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39\r\n"))                         flag = false;
    if (!nmea.parse("$GPGSV,2,1,08,01,40,083,46,02,17,308,41,12,07,344,39,14,22,228,45*75\r\n"))    flag = false;
    if (!nmea.parse("$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A\r\n"))    flag = false;
    if (!nmea.parse("$$GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39\r\n$GPGSV,2,1,08,01,40,083")) flag = false;
    if (!nmea.parse(",46,02,17,308,41,12,07,344,39,14,22,228,45*75\r\n$GPRMC,034116.00,A"))         flag = false;

    {
        Nmea0183Message *msg = nmea.getNextMessage();
        if (msg == NULL) {
            flag = false;
        } else if (msg->isMessageType("GGA")) {
            Nmea0183GGA *gga = static_cast <Nmea0183GGA *> (msg);
            if (fabs(gga->latitude_degN  -  48.1173) > 0.0001)  flag = false;
            if (fabs(gga->longitude_degE -  11.5167) > 0.0001)  flag = false;
            if (     gga->gpsType                   != 1)       flag = false;
            if (     gga->numSatelites              != 8)       flag = false;
            if (fabs(gga->hdop           -   0.9)    > 0.01)    flag = false;
            if (fabs(gga->altitude_m     - 545.4)    > 0.01)    flag = false;
        } else {
            flag = false;
        }
    }

    {
        Nmea0183Message *msg = nmea.getNextMessage();
        if (msg == NULL) {
            flag = false;
        } else if (msg->isMessageType("RMC")) {
            Nmea0183RMC *rmc = static_cast <Nmea0183RMC *> (msg);
            if (fabs(rmc->latitude_degN  -  48.1173) > 0.0001)  flag = false;
            if (fabs(rmc->longitude_degE -  11.5167) > 0.0001)  flag = false;
            if (fabs(rmc->speed_mps - 22.4 * KNOTS_TO_METERS_PER_SECOND) > 0.01)    flag = false;
            if (fabs(rmc->course_rad            -  84.4 * M_PI/180)      > 0.01)    flag = false;
            if (fabs(rmc->magneticVariation_rad -  -3.1 * M_PI/180)      > 0.01)    flag = false;
        } else {
            flag = false;
        }
    }

    std::cout << "Nmea0183Parser.unitTest " <<
        ((flag) ? "Passed" : "Failed") << std::endl;

    return flag;
}
